#include "StrikeForceAlphaState.h"
#include "TargetNearestTankState.h"
#include "Taunts.h"

bool alpha_first_visit = true;
Vector2 alpha_first_waypoint(Agent* agent, bool* stateChanged);
Vector2 alpha_second_waypoint(Agent* agent, bool* stateChanged);
Vector2 alpha_third_waypoint(Agent* agent, bool* stateChanged);
Vector2 alpha_grab_flag(Agent* agent, bool* stateChanged);
Vector2 alpha_come_home(Agent* agent, bool* stateChanged);

Vector2 alpha_first_waypoint(Agent* agent, bool* stateChanged) {
	World* world = World::instance();
	string enemy_color = world->other_tanks()[0].color;

	Vector2 my_base_pos = world->getBasePos(world->getTeam());
	Vector2 their_base_pos = world->getBasePos(enemy_color);

	float mag = (my_base_pos - their_base_pos).magnitude();
	Vector2 dir = (their_base_pos - my_base_pos).normalize();
	Vector2 up = (my_base_pos - their_base_pos).orthogonal();

	Vector2 waypoint = my_base_pos + dir*(mag/5.0f) + up * ((world->getSize() / 2) - 100);
	if(fabs(waypoint.x) > (world->getSize() / 2) - 50){
		if(waypoint.x < 0)
			waypoint.x = -(world->getSize() / 2) + 50;
		else
			waypoint.x = (world->getSize() / 2) - 50;
	}
	if(fabs(waypoint.y) > (world->getSize() / 2) - 50){
		if(waypoint.y < 0)
			waypoint.y = -(world->getSize() / 2) + 50;
		else
			waypoint.y = (world->getSize() / 2) - 50;
	}

	return waypoint;
	//return my_base_pos + up * ((world->getSize() / 2) - 100);
}

Vector2 alpha_second_waypoint(Agent* agent, bool* stateChanged) {
	if (alpha_first_visit) {
		alpha_first_visit = false;
		Taunts::queue.push("Lets try some sneaky stuff");
	}

	World* world = World::instance();
	string enemy_color = world->other_tanks()[0].color;

	Vector2 my_base_pos = world->getBasePos(world->getTeam());
	Vector2 their_base_pos = world->getBasePos(enemy_color);

	float mag = (my_base_pos - their_base_pos).magnitude();
	Vector2 dir = (their_base_pos - my_base_pos).normalize();
	Vector2 up = (my_base_pos - their_base_pos).orthogonal();

	Vector2 waypoint = my_base_pos + dir*(mag/2.0f) + up * ((world->getSize() / 2) - 100);
	if(fabs(waypoint.x) > (world->getSize() / 2) - 50){
		if(waypoint.x < 0)
			waypoint.x = -(world->getSize() / 2) + 50;
		else
			waypoint.x = (world->getSize() / 2) - 50;
	}
	if(fabs(waypoint.y) > (world->getSize() / 2) - 50){
		if(waypoint.y < 0)
			waypoint.y = -(world->getSize() / 2) + 50;
		else
			waypoint.y = (world->getSize() / 2) - 50;
	}

	return waypoint;
}

Vector2 alpha_third_waypoint(Agent* agent, bool* stateChanged) {
	World* world = World::instance();
	string enemy_color = world->other_tanks()[0].color;

	Vector2 my_base_pos = world->getBasePos(world->getTeam());
	Vector2 their_base_pos = world->getBasePos(enemy_color);

	float mag = (my_base_pos - their_base_pos).magnitude();
	Vector2 dir = (their_base_pos - my_base_pos).normalize();
	Vector2 up = (my_base_pos - their_base_pos).orthogonal();

	Vector2 waypoint = my_base_pos + dir*(4.0f*mag/5.0f) + up * ((world->getSize() / 2) - 100);
	if(fabs(waypoint.x) > (world->getSize() / 2) - 50){
		if(waypoint.x < 0)
			waypoint.x = -(world->getSize() / 2) + 50;
		else
			waypoint.x = (world->getSize() / 2) - 50;
	}
	if(fabs(waypoint.y) > (world->getSize() / 2) - 50){
		if(waypoint.y < 0)
			waypoint.y = -(world->getSize() / 2) + 50;
		else
			waypoint.y = (world->getSize() / 2) - 50;
	}

	return waypoint;
}

Vector2 alpha_grab_flag(Agent* agent, bool* stateChanged) {
	World* world = World::instance();
	string enemy_color = world->other_tanks()[0].color;
	return world->getFlagPos(enemy_color);
}

Vector2 alpha_come_home(Agent* agent, bool* stateChanged) {
	World* world = World::instance();
	return world->getFlagPos(world->getTeam());
}

StrikeForceAlphaState::~StrikeForceAlphaState(void) {
}

void StrikeForceAlphaState::add_waypoint_methods() {
	add_waypoint_method(alpha_first_waypoint);
	add_waypoint_method(alpha_second_waypoint);
	add_waypoint_method(alpha_third_waypoint);
	add_waypoint_method(alpha_grab_flag);
	add_waypoint_method(alpha_third_waypoint);
	add_waypoint_method(alpha_second_waypoint);
	add_waypoint_method(alpha_first_waypoint);
	add_waypoint_method(alpha_come_home);
}